#include "QuadTree.h"

namespace poly
{
	QuadTree::QuadTree( int capacity ) :
		root( 0 ), m_capacity( capacity )
	{
	}

	QuadTree::QuadTree( float xmin, float xmax, float ymin, float ymax, int depth )
	{
		root = new Node( xmin, xmax, ymin, ymax );
	}

	void QuadTree::insert( QuadTree::IObject * object )
	{
		root->insert( object, m_capacity );
		while( root->parent != 0 )
			root = root->parent;
	}

	QuadTree::Node::Node( float _xmin, float _xmax, float _ymin, float _ymax ) :
		xmin(_xmin), xmax(_xmax), ymin(_ymin), ymax(_ymax),
		parent( 0 ), left_top( 0 ), left_bottom( 0 ), right_top( 0 ), right_bottom( 0 )
	{
	}


	// QuadTree Insertion Methods
	void QuadTree::Node::insert( IObject * object, int capacity )
	{
		Span span = object->span();
		insert( object, span, capacity );
	}

	void QuadTree::Node::insert( IObject * object, Span const & span, int capacity )
	{
		// if outside node, insert object into node's parent
		if( span.xmax < xmin || span.xmin > xmax ||
			span.ymax < ymin || span.ymin > ymax )
		{
			// if is top node, enlarge tree above current node
			if( parent == 0 )
			{
				float nxmin, nxmax, nymin, nymax;
				if( span.xmin < xmin ) // enlarge to the left
				{
					nxmin = 2 * xmin - xmax;
					nxmax = xmax;
				}
				else if( span.xmax > xmax ) // enlarge to the right
				{
					nxmax = 2 * xmax - xmin;
					nxmin = xmin;
				}

				if( span.ymin < ymin ) // enlarge to the bottom
				{
					nymin = 2 * ymin - ymax;
					nymax = ymax;
				}
				else if( span.ymax > ymax ) // enlarge to the top
				{
					nymax = 2 * ymax - ymin;
					nymin = ymin;
				}
				
				parent = new Node( nxmin, nxmax, nymin, nymax );
			}
			parent->insert( object, capacity );
			return;
		}

		// if node is going to overflow with this object
		if( objects.size() >= capacity )
		{
			// check if object must be placed in this node
			float xmid = (xmax + xmin)/2;
			float ymid = (ymax + ymin)/2;
			if( span.xmin < xmid && span.ymax > xmid ||
				span.ymin < ymid && span.ymax > ymid )
			{
				objects.insert( object );
				object->node = this;
			}
			else // or placed in node's children
			{
				insertIntoChildren( object, span, capacity, xmid, ymid );
			}

			// then reinsert the node's content into its children
			ObjectSet objects_old = objects;
			objects.clear();
			for( ObjectSet::iterator itr = objects_old.begin();
				itr != objects_old.end(); itr++ )
			{
				insert( *itr, 0 ); // insert object at zero capacity
			}
		}
		else // if no overflow, just add the object
		{
			objects.insert( object );
			object->node = this;
		}
	}

	// span assumed not to intersect parent's mid lines
	void QuadTree::Node::insertIntoChildren( IObject * object, Span const & span, int capacity, float xmid, float ymid )
	{
		bool left = span.xmax < (xmin + xmax)/2;
		bool bottom = span.ymax < (ymin + ymax)/2;

		if( left )
			if( bottom )
			{
				if( left_bottom == 0 )
				{
					left_bottom = new Node( span.xmin, xmid, span.ymin, ymid );
					left_bottom->parent = this;
				}
				left_bottom->insert( object, capacity );
			}
			else
			{
				if( left_top == 0 )
				{
					left_top = new Node( span.xmin, xmid, ymid, span.ymax );
					left_top->parent = this;
				}
				left_top->insert( object, capacity );
			}
		else
			if( bottom )
			{
				if( right_bottom == 0 )
				{
					right_bottom = new Node( xmid, span.xmax, span.ymin, ymid );
					right_bottom->parent = this;
				}
				right_bottom->insert( object, capacity );
			}
			else
			{
				if( right_top == 0 )
				{
					right_top = new Node( xmid, span.xmax, ymid, span.ymax );
					right_top->parent = this;
				}
				right_top->insert( object, capacity );
			}
	}


	// QuadTree Removment Methods
	void QuadTree::remove( QuadTree::IObject * object )
	{
		object->node->objects.erase( object );
	}

	void QuadTree::clean( Node * node )
	{
		while( node != 0 )
		{
			// if node is empty
			if( node->left_bottom == 0 &&
				node->left_top == 0 &&
				node->right_bottom == 0 &&
				node->right_top == 0 &&
				node->objects.size() == 0 )
			{
				// remove from parent
				Node * parent = node->parent;
				if( parent != 0 )
				{
					if( parent->left_bottom == node )
						parent->left_bottom = 0;
					else if( parent->left_top == node )
						parent->left_top = 0;
					else if( parent->right_bottom == node )
						parent->right_bottom = 0;
					else if( parent->left_top == node )
						parent->left_top = 0;
				}

				delete node;

				node = parent; // used for next iteration
			}
			else
				return;
		}
	}
}
